#include "Angles.h"
#include "Quaternions.h"
#include "Rotation.h"
#include "Math.h"
#include "Vector3.h"
#include "Matrix3.h"
#include "Matrix4.h"
namespace Core
{
	rtAngles ang_zero(0.0f, 0.0f, 0.0f);

	inline rtAngles::rtAngles(void) 
	{
	}

	inline rtAngles::rtAngles(float pitch, float yaw, float roll) 
	{
		this->pitch = pitch;
		this->yaw	= yaw;
		this->roll	= roll;
	}

	inline rtAngles::rtAngles(const rtVector3& v) 
	{
		this->pitch = v[0];
		this->yaw	= v[1];
		this->roll	= v[2];
	}

	inline void rtAngles::set(float pitch, float yaw, float roll) 
	{
		this->pitch = pitch;
		this->yaw	= yaw;
		this->roll	= roll;
	}

	inline rtAngles& rtAngles::zero(void) 
	{
		pitch = yaw = roll = 0.0f;
		return *this;
	}

	inline float rtAngles::operator [] (int index) const 
	{
		assert((index >= 0) && (index < 3));
		return (&pitch)[index];
	}

	inline float& rtAngles::operator [] (int index) 
	{
		assert((index >= 0 ) && ( index < 3));
		return (&pitch)[index];
	}

	inline rtAngles rtAngles::operator - () const 
	{
		return rtAngles(-pitch, -yaw, -roll);
	}

	inline rtAngles& rtAngles::operator = (const rtAngles &a) 
	{
		pitch	= a.pitch;
		yaw		= a.yaw;
		roll	= a.roll;
		return *this;
	}

	inline rtAngles rtAngles::operator + (const rtAngles& a) const 
	{
		return rtAngles(pitch + a.pitch, yaw + a.yaw, roll + a.roll);
	}

	inline rtAngles& rtAngles::operator += (const rtAngles& a) 
	{
		pitch	+= a.pitch;
		yaw		+= a.yaw;
		roll	+= a.roll;

		return *this;
	}

	inline rtAngles rtAngles::operator - (const rtAngles& a) const 
	{
		return rtAngles(pitch - a.pitch, yaw - a.yaw, roll - a.roll);
	}

	inline rtAngles& rtAngles::operator -= (const rtAngles& a) 
	{
		pitch	-= a.pitch;
		yaw		-= a.yaw;
		roll	-= a.roll;

		return *this;
	}

	inline rtAngles rtAngles::operator * (const float a) const 
	{
		return rtAngles(pitch * a, yaw * a, roll * a);
	}

	inline rtAngles& rtAngles::operator *= (float a) 
	{
		pitch	*= a;
		yaw		*= a;
		roll	*= a;
		return *this;
	}

	inline rtAngles rtAngles::operator / (const float a) const 
	{
		float inva = 1.0f / a;
		return rtAngles(pitch * inva, yaw * inva, roll * inva);
	}

	inline rtAngles& rtAngles::operator /= (float a) 
	{
		float inva = 1.0f / a;
		pitch	*= inva;
		yaw		*= inva;
		roll	*= inva;
		return *this;
	}

	inline rtAngles operator * (const float a, const rtAngles& b) 
	{
		return rtAngles(a * b.pitch, a * b.yaw, a * b.roll);
	}

	inline bool rtAngles::compare(const rtAngles& a) const 
	{
		return ((a.pitch == pitch) && (a.yaw == yaw) && (a.roll == roll));
	}

	inline bool rtAngles::compare(const rtAngles& a, const float epsilon) const 
	{
		if (rtMath::Fabs(pitch - a.pitch) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(yaw - a.yaw) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(roll - a.roll) > epsilon) 
		{
			return false;
		}

		return true;
	}

	inline bool rtAngles::operator == (const rtAngles& a) const 
	{
		return compare(a);
	}

	inline bool rtAngles::operator != (const rtAngles& a) const 
	{
		return !compare(a);
	}

	inline void rtAngles::clamp(const rtAngles& min, const rtAngles& max) 
	{
		if(pitch < min.pitch) 
		{
			pitch = min.pitch;
		} 
		else if(pitch > max.pitch) 
		{
			pitch = max.pitch;
		}

		if (yaw < min.yaw) 
		{
			yaw = min.yaw;
		} 
		else if (yaw > max.yaw) 
		{
			yaw = max.yaw;
		}

		if (roll < min.roll) 
		{
			roll = min.roll;
		} 
		else if (roll > max.roll) 
		{
			roll = max.roll;
		}
	}

	inline int rtAngles::getDimension(void) const 
	{
		return 3;
	}

	inline const float* rtAngles::toFloatPtr(void) const 
	{
		return &pitch;
	}

	inline float* rtAngles::toFloatPtr(void) 
	{
		return &pitch;
	}

	rtAngles& rtAngles::normalize360(void) 
	{
		int i;

		for (i = 0; i < 3; i++) 
		{
			if(((*this)[i] >= 360.0f) || ((*this)[i] < 0.0f)) 
			{
				(*this)[i] -= floor((*this)[i] / 360.0f) * 360.0f;

				if((*this)[i] >= 360.0f) 
				{
					(*this)[i] -= 360.0f;
				}
				if((*this)[i] < 0.0f) 
				{
					(*this)[i] += 360.0f;
				}
			}
		}

		return *this;
	}


	rtAngles& rtAngles::normalize180(void) 
	{
		normalize360();

		if(pitch > 180.0f) 
		{
			pitch -= 360.0f;
		}

		if(yaw > 180.0f) 
		{
			yaw -= 360.0f;
		}

		if(roll > 180.0f) 
		{
			roll -= 360.0f;
		}
		return *this;
	}


	void rtAngles::toVectors(rtVector3* forward, rtVector3* right, rtVector3* up) const 
	{
		float sr, sp, sy, cr, cp, cy;

		rtMath::SinCos(DEG2RAD(yaw), sy, cy);
		rtMath::SinCos(DEG2RAD(pitch), sp, cp);
		rtMath::SinCos(DEG2RAD(roll), sr, cr);

		if(forward) 
		{
			forward->set(cp * cy, cp * sy, -sp);
		}

		if(right) 
		{
			right->set(-sr * sp * cy + cr * sy, -sr * sp * sy + -cr * cy, -sr * cp);
		}

		if(up) 
		{
			up->set(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp);
		}
	}


	rtVector3 rtAngles::toForward(void) const 
	{
		float sp, sy, cp, cy;

		rtMath::SinCos(DEG2RAD(yaw), sy, cy);
		rtMath::SinCos(DEG2RAD(pitch), sp, cp);

		return rtVector3(cp * cy, cp * sy, -sp);
	}


	rtQuaternions rtAngles::toQuat(void) const 
	{
		float sx, cx, sy, cy, sz, cz;
		float sxcy, cxcy, sxsy, cxsy;

		rtMath::SinCos(DEG2RAD(yaw) * 0.5f, sz, cz);
		rtMath::SinCos(DEG2RAD(pitch) * 0.5f, sy, cy);
		rtMath::SinCos(DEG2RAD(roll) * 0.5f, sx, cx);

		sxcy = sx * cy;
		cxcy = cx * cy;
		sxsy = sx * sy;
		cxsy = cx * sy;

		return rtQuaternions(cxsy * sz - sxcy * cz, -cxsy * cz - sxcy * sz, sxsy * cz - cxcy * sz, cxcy * cz + sxsy * sz);
	}


	rtRotation rtAngles::toRotation(void) const 
	{
		rtVector3 vec;
		float angle, w;
		float sx, cx, sy, cy, sz, cz;
		float sxcy, cxcy, sxsy, cxsy;

		if (pitch == 0.0f) 
		{
			if(yaw == 0.0f) 
			{
				return rtRotation(rtVector3::vec3_origin, rtVector3(-1.0f, 0.0f, 0.0f), roll);
			}
			if(roll == 0.0f) 
			{
				return rtRotation(rtVector3::vec3_origin, rtVector3(0.0f, 0.0f, -1.0f), yaw);
			}
		} 
		else if(yaw == 0.0f && roll == 0.0f) 
		{
			return rtRotation(rtVector3::vec3_origin, rtVector3(0.0f, -1.0f, 0.0f), pitch);
		}

		rtMath::SinCos(DEG2RAD(yaw) * 0.5f, sz, cz);
		rtMath::SinCos(DEG2RAD(pitch) * 0.5f, sy, cy);
		rtMath::SinCos(DEG2RAD(roll) * 0.5f, sx, cx);

		sxcy = sx * cy;
		cxcy = cx * cy;
		sxsy = sx * sy;
		cxsy = cx * sy;

		vec.x =  cxsy * sz - sxcy * cz;
		vec.y = -cxsy * cz - sxcy * sz;
		vec.z =  sxsy * cz - cxcy * sz;
		w =		 cxcy * cz + sxsy * sz;
		angle = rtMath::ACos(w);
		if (angle == 0.0f) 
		{
			vec.set(0.0f, 0.0f, 1.0f);
		} 
		else 
		{
			vec.normalize();
			vec.fixDegenerateNormal();
			angle *= 2.0f * rtMath::M_RAD2DEG;
		}
		return rtRotation(rtVector3::vec3_origin, vec, angle);
	}


	rtMatrix3 rtAngles::toMat3( void ) const 
	{
		rtMatrix3 mat;
		float sr, sp, sy, cr, cp, cy;

		rtMath::SinCos(DEG2RAD(yaw), sy, cy);
		rtMath::SinCos(DEG2RAD(pitch), sp, cp);
		rtMath::SinCos(DEG2RAD(roll), sr, cr);

		mat[0].set(cp * cy, cp * sy, -sp);
		mat[1].set(sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp);
		mat[2].set(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp);

		return mat;
	}


	rtMatrix4 rtAngles::toMat4(void) const 
	{
		return toMat3().toMat4();
	}


	rtVector3 rtAngles::toAngularVelocity(void) const 
	{
		rtRotation rotation = toRotation();
		return rotation.getVec() * DEG2RAD(rotation.getAngle());
	}


}